from point import Point
from rob import Rob
from mechanism import Mechanism
import sympy as sp
from sympy.abc import t

class RRR(Mechanism):
  """
  Attrs:
    p0：内副C点
    p1:外接副1，B点
    p2：外接副2：D点
    rob1：BC杆
    rob2：CD杆
    phi1：以B基点，CB倾角
    phi2：D为基点，CD倾角
    clockwise：BCD是否顺时针
  """
  def __init__(self,p1:Point,p2:Point,l1,l2,clockwise = True):
    """
    Args: 
        p1:外接副1
        p2:外接副2
        l1,l2:杆长
        clockwise:bcd顺时针排列
    """
    # 确保2点不是重合的
    assert p1.symbol_rx != p2.symbol_rx or p1.symbol_ry != p2.symbol_ry
    print('初始化RRR杆组')

    self.p1 = p1;self.p2 = p2
    self.l1 = l1;self.l2 = l2
    self.clockwise = clockwise

    a = 2*l1*(p2.symbol_rx - p1.symbol_rx)
    b = 2*l1*(p2.symbol_ry - p1.symbol_ry)
    lbd_2 = (p2.symbol_rx - p1.symbol_rx)**2 + (p2.symbol_ry - p1.symbol_ry)**2# BD长度^2

    c = l1**2-l2**2 +lbd_2

    self.non_minus = sp.lambdify(t,a**2+b**2-c**2,'numpy')
    assert self.non_minus(0)>=0 ,"RRR无法装配：a方 + b方 - c方必须大于0"

    if(clockwise):self.symbol_phi1 = 2*sp.atan((b+sp.sqrt(a**2+b**2-c**2))/(a+c))
    else:self.symbol_phi1 = 2*sp.atan((b-sp.sqrt(a**2+b**2-c**2))/(a+c))
    
    self.p0 = Point(rx = p1.symbol_rx + l1*sp.cos(self.symbol_phi1),ry=p1.symbol_ry+l1*sp.sin(self.symbol_phi1))
    self.symbol_phi2 = sp.pi + sp.atan((self.p0.symbol_ry - self.p2.symbol_ry)/(self.p0.symbol_rx-self.p2.symbol_rx))

    self.rob1 = Rob(l1,self.p1,self.symbol_phi1,endPoint=self.p0)
    self.rob2 = Rob(l2,self.p2,self.symbol_phi2,endPoint=self.p0)

    print('RRR杆组初始化完成')

  def get_robs(self):
    return self.rob1,self.rob2
  
  def update(self,t_s):
    assert self.non_minus(t_s)>=0 ,"RRR无法装配：a方 + b方 - c方必须大于0"
    self.rob1.update(t_s)
    self.rob2.update(t_s)
  
  



if __name__=="__main__":
  p1 = Point(0,0)
  p2 = Point(50+t,0)
  
  m = RRR(p1,p2,100,50)